package simulator;
import java.util.ArrayList;
import java.awt.Graphics;
import java.awt.Color;
/**
 * Detects collision with walls, imitating the behavior of the 
 * dual bumpsensors on the iRobot Create
 * @author anand
 *
 */
public class BumpSensor extends Sensor{
  public static final double PRESSED = 1.0;
  public static final double UNPRESSED = 0.0;
  public static final int LEFT = 0;
  public static final int RIGHT = 1;
  private double mountangle;
  //private static final double TOLERANCE = 180.0;  //degrees of tolerance in contact
  public BumpSensor(Environment e, double x, double y){
    super(e);
    this.x=x;
    this.y=y;
    mountangle=0;
  }
  
  public BumpSensor(Environment e, double x, double y, double angle){
    super(e);
    this.x=x;
    this.y=y;
    mountangle=Math.toRadians(angle);
  }
    
  public double[] read(int magicnum) {  
    double[] reading = new double[3];
	reading[0] = UNPRESSED;
	reading[1] = UNPRESSED;
    ArrayList<PhysicalObject> known=env.getObjects();
	
	for(PhysicalObject o : known){
	  for(int i=0; i<4; i++) {
	    if(o.intersects(env.getRobotX() + ((x + i) * Math.cos(env.getRobotAngle() +Math.toRadians(15))),
	                  env.getRobotY() + ((y + i) * Math.sin(env.getRobotAngle() +Math.toRadians(15)))))
	      reading[LEFT] = PRESSED;
	    if(o.intersects(env.getRobotX() + ((x + i) * Math.cos(env.getRobotAngle() -Math.toRadians(15))),
                env.getRobotY() + ((y + i) * Math.sin(env.getRobotAngle() -Math.toRadians(15)))))
	      reading[RIGHT] = PRESSED;
	  }
	}
	reading[2] = Simulator.time;
	setState(reading);
	return reading;
  }

  public void paint(Graphics g) {
    g.setColor(Color.BLUE);
    g.drawLine((int)(env.getRobotX() + ((x + 0) * Math.cos(env.getRobotAngle()))),
	       (int)(env.getRobotY() + ((y + 0) * Math.sin(env.getRobotAngle()))),
	       (int)(env.getRobotX() + ((x + 5) * Math.cos(env.getRobotAngle()))),
	       (int)(env.getRobotY() + ((y + 5) * Math.sin(env.getRobotAngle()))));
    
  }
  
  public String getName(){
    return "TouchSensor";  //add id as well?
  }
  
  public int getID() {
      return 2;
  }
  
  public double getMountAngle(){
    return mountangle;
  }

}